#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <iostream>
#include <random>
#include <cmath>
using namespace std;

class BSplineCurve{
    public:
        BSplineCurve();
        ~BSplineCurve() {};

        // vector<geometry_msgs::Point> -> nav_msgs::Path
        nav_msgs::Path ToPath(vector<geometry_msgs::Point> points, string frame_id);
        // vector<geometry_msgs::Point> -> sensor_msgs::PointCloud2
        sensor_msgs::PointCloud2 ToPointCloud2(vector<geometry_msgs::Point> points, string frame_id);

        // 计算B样条基函数
        double N(int i, int p, double u, const std::vector<double>& knots);
        // 计算B样条曲线的点
        geometry_msgs::Point BSpline(int p, const std::vector<geometry_msgs::Point>& controlPoints, const std::vector<double>& knots, double u);

        void Test();

    private:
        ros::NodeHandle __nh;
        ros::Publisher __line_pub;
        ros::Publisher __points_pub;
        vector<geometry_msgs::Point> __points;        
};

BSplineCurve::BSplineCurve(){
    __line_pub = __nh.advertise<nav_msgs::Path>("/bspline_curve", 1, true);
    __points_pub = __nh.advertise<sensor_msgs::PointCloud2>("/bspline_keypoints", 1, true);
    ros::Duration(1.0).sleep();      
}

// vector<geometry_msgs::Point> -> nav_msgs::Path
nav_msgs::Path BSplineCurve::ToPath(vector<geometry_msgs::Point> points, string frame_id){
    nav_msgs::Path path;
    path.header.frame_id = frame_id;
    path.header.stamp = ros::Time();    
    for(size_t i=0; i<points.size(); ++i){
        geometry_msgs::Pose pose_msg;
        pose_msg.position.x = points[i].x;
        pose_msg.position.y = points[i].y;
        pose_msg.position.z = points[i].z;
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header.frame_id = frame_id;
        pose_stamp.header.stamp = ros::Time();
        pose_stamp.pose = pose_msg;
        path.poses.emplace_back(pose_stamp);
    }
    return path;
}

// vector<geometry_msgs::Point> -> sensor_msgs::PointCloud2
sensor_msgs::PointCloud2 BSplineCurve::ToPointCloud2(vector<geometry_msgs::Point> points, string frame_id){
    sensor_msgs::PointCloud2 cloud_msg;
    cloud_msg.header.frame_id = frame_id;
    cloud_msg.header.stamp = ros::Time::now();
    cloud_msg.height = 1;
    cloud_msg.width = points.size();
    cloud_msg.is_dense = false;

    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(points.size());

    // 使用迭代器填充点云消息的数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
    for(const auto& point : points) {
        *iter_x = point.x;
        *iter_y = point.y;
        *iter_z = point.z;

        ++iter_x;
        ++iter_y;
        ++iter_z;
    }    
    return cloud_msg;    
}
double BSplineCurve::N(int i, int p, double u, const std::vector<double>& knots) {
    if (p == 0) {
        // 0阶基函数，B样条基函数为单位脉冲函数
        if (knots[i] <= u && u < knots[i + 1]) {
            return 1.0;
        }
        return 0.0;
    } else {
        // 递归计算B样条基函数
        double left = 0.0, right = 0.0;
        
        // 防止除以0的错误，计算前面的基函数部分
        if(knots[i + p] - knots[i] == 0.0){
            left =  0.0;
        }
        else{
            left = (u - knots[i]) / (knots[i + p] - knots[i]);
        }
        
        // 防止除以0的错误，计算后面的基函数部分
        if(knots[i + p + 1] - knots[i + 1] == 0.0){
            right =  0.0;
        }
        else{
            right = (knots[i + p + 1] - u) / (knots[i + p + 1] - knots[i + 1]);
        }
        
        return left * N(i, p - 1, u, knots) + right * N(i + 1, p - 1, u, knots);
    }
}

// 计算B样条曲线的点
geometry_msgs::Point BSplineCurve::BSpline(int p, const std::vector<geometry_msgs::Point>& controlPoints, const std::vector<double>& knots, double u) {
    geometry_msgs::Point curvePoint;
    curvePoint.x = 0.0;
    curvePoint.y = 0.0;
    curvePoint.z = 0.0;  // 如果是3D路径，z坐标需要加权累积

    int n = controlPoints.size() - 1; // 控制点的数量 - 1

    // 确保u在有效的范围内
    if (u < knots[p] || u > knots[n + 1]) {
        ROS_WARN("u value out of range: %f, should be in [%f, %f]", u, knots[p], knots[n + 1]);
        return curvePoint; // 如果u不在有效范围内，返回当前默认的curvePoint
    }

    // 计算B样条曲线上的点
    for (int i = 0; i <= n; ++i) {
        double basis = N(i, p, u, knots);  // 计算第i个基函数的值
        cout << "[BSpline] i: " << i << ", basis: " << basis << ", x: " << controlPoints[i].x
            << ", y: " << controlPoints[i].y 
            << ", p: " << p << ", u: " << u << endl;
        curvePoint.x += basis * controlPoints[i].x;  // X坐标
        curvePoint.y += basis * controlPoints[i].y;  // Y坐标
        curvePoint.z += basis * controlPoints[i].z;  // 如果是3D曲线，Z坐标也需要加权累积
    }

    return curvePoint;
}

void BSplineCurve::Test(){

    // 控制点集（使用geometry_msgs::Point类型）
    geometry_msgs::Point point;
    // 控制点 1
    point.x = 0.0;
    point.y = 0.0;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 2
    point.x = 1.0;
    point.y = 2.0;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 3
    point.x = 2.0;
    point.y = 3.0;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 4
    point.x = 3.0;
    point.y = 2.5;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 5
    point.x = 4.0;
    point.y = 1.5;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 6
    point.x = 5.0;
    point.y = 1.0;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 7
    point.x = 6.0;
    point.y = 1.5;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 8
    point.x = 7.0;
    point.y = 3.0;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 9
    point.x = 8.0;
    point.y = 4.0;
    point.z = 0.0;
    __points.emplace_back(point);

    // 控制点 10
    point.x = 9.0;
    point.y = 5.0;
    point.z = 0.0;
    __points.emplace_back(point);


    // 发布点云数据
    sensor_msgs::PointCloud2 cloud_msg = ToPointCloud2(__points, "map");
    __points_pub.publish(cloud_msg);
    
    // // 节点向量（阶数为3的B样条，节点向量前后各重复3次）
    std::vector<double> knots;
    
    // 曲线的阶数 p
    int p = 3;
    
    vector<geometry_msgs::Point> curve_points;
    // 计算曲线在 u = 1 到 u = 6 之间的点，包含起点和终点

    // 均匀分布
    int n = __points.size() -1;
    double dis_u = 1.0 / (n + p);
    for(int i=0; i <n+p+1; i++){
        knots.emplace_back(i * dis_u);
    }

    double u_start =  knots[p - 1];
    double u_end =  knots[n + 1];

    for (double u = u_start; u <= u_end; u += 0.01){
        geometry_msgs::Point point = BSpline(p, __points, knots, u);
        curve_points.emplace_back(point);
        std::cout << "--> Point: (" << point.x << ", " << point.y << ", " << point.z << ")" << std::endl;
    }


    nav_msgs::Path path_msg = ToPath(curve_points, "map");
    __line_pub.publish(path_msg);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "test_b_spline");
    ROS_INFO("test_b_spline initializating......");
    ros::NodeHandle nh;
    BSplineCurve b_spline;
    b_spline.Test();
    ros::spin();
    return 0;
}


